#include <avr/io.h>
#include <avr/delay.h>

#include "power.h"
#include "RGB_LED.h"
#include "motor.h"

cap_monitor_init()
{
	PORTB.DIRCLR = PIN0_bm | PIN1_bm;
	
	ACB.AC0CTRL = AC_ENABLE_bm;
	ACB.AC1CTRL = AC_ENABLE_bm;
	
	ACB.AC0MUXCTRL = AC_MUXPOS_PIN1_gc | AC_MUXNEG_BANDGAP_gc;
	ACB.AC1MUXCTRL = AC_MUXPOS_PIN1_gc | AC_MUXNEG_PIN0_gc;
	
	ACB.WINCTRL = AC_WEN_bm;
}

uint8_t cap_status()
{
	switch (ACB.STATUS & AC_WSTATE_gm)
	{
		case AC_WSTATE_ABOVE_gc:  return 1;
		case AC_WSTATE_INSIDE_gc: return 0;
		case AC_WSTATE_BELOW_gc:  return -1;
	}
	return -2;
}

void leg_monitor_init()
{
	PORTA.DIRCLR = PIN0_bm | PIN1_bm | PIN2_bm | PIN3_bm | PIN4_bm;
	
	ACA.AC0CTRL = AC_HSMODE_bm | AC_ENABLE_bm;
	ACA.AC1CTRL = AC_HSMODE_bm | AC_ENABLE_bm;
	
	ACA.AC0MUXCTRL = AC_MUXNEG_PIN0_gc;			// PA0 is VREF_HI
	ACA.AC1MUXCTRL = AC_MUXNEG_PIN1_gc;			// PA1 is VREF_LO
	
	ACA.AC0MUXCTRL |= AC_MUXPOS_PIN2_gc;		// Initially, look at leg1
	ACA.AC1MUXCTRL |= AC_MUXPOS_PIN2_gc;
	
	ACA.WINCTRL = AC_WEN_bm;					// Enable window mode
}

int8_t leg1_status()
{
	ACA.AC0MUXCTRL = AC_MUXNEG_PIN0_gc | AC_MUXPOS_PIN2_gc;
	ACA.AC1MUXCTRL = AC_MUXNEG_PIN1_gc | AC_MUXPOS_PIN2_gc;
	
	_delay_ms(1);
	
	uint8_t status = ACA.STATUS;
	
	if ((status & AC_WSTATE_gm) == AC_WSTATE_ABOVE_gc) { return 1; }
	if ((status & AC_WSTATE_gm) == AC_WSTATE_INSIDE_gc) { return 0; }
	if ((status & AC_WSTATE_gm) == AC_WSTATE_BELOW_gc) { return -1; }
	return -2;
}

int8_t leg2_status()
{
	ACA.AC0MUXCTRL = AC_MUXNEG_PIN0_gc | AC_MUXPOS_PIN3_gc;
	ACA.AC1MUXCTRL = AC_MUXNEG_PIN1_gc | AC_MUXPOS_PIN3_gc;

	_delay_ms(1);
	
	uint8_t status = ACA.STATUS;
	
	if ((status & AC_WSTATE_gm) == AC_WSTATE_ABOVE_gc) { return 1; }
	if ((status & AC_WSTATE_gm) == AC_WSTATE_INSIDE_gc) { return 0; }
	if ((status & AC_WSTATE_gm) == AC_WSTATE_BELOW_gc) { return -1; }
	return -2;
}

int8_t leg3_status()
{
	ACA.AC0MUXCTRL = AC_MUXNEG_PIN0_gc | AC_MUXPOS_PIN4_gc;
	ACA.AC1MUXCTRL = AC_MUXNEG_PIN1_gc | AC_MUXPOS_PIN4_gc;

	_delay_ms(1);
	
	uint8_t status = ACA.STATUS;
	
	if ((status & AC_WSTATE_gm) == AC_WSTATE_ABOVE_gc) { return 1; }
	if ((status & AC_WSTATE_gm) == AC_WSTATE_INSIDE_gc) { return 0; }
	if ((status & AC_WSTATE_gm) == AC_WSTATE_BELOW_gc) { return -1; }
	return -2;
}

int8_t leg_status(uint8_t leg)
{
	switch (leg)
	{
		case 1:
			return leg1_status();
		case 2:
			return leg2_status();
		case 3:
			return leg3_status();
	}		
	return 0x80;		
}

uint8_t findPower()
{
	uint8_t duty_cycle = 192, onTime = 200, offTime = 200;
	
	uint8_t mot = 1, hadPower = 1;
	uint8_t oldtcc0cca = TCC0_CCA;
	uint8_t oldtcc0ccb = TCC0_CCB;
	uint8_t oldtcc1cca = TCC1_CCA;
	uint8_t oldtcc1ccb = TCC1_CCB;
	uint8_t oldtce0cca = TCE0_CCA;
	uint8_t oldtce0ccb = TCE0_CCB;
	
	TCC0_CCA = TCC0_CCB = duty_cycle;
	TCC1_CCA = TCC1_CCB = duty_cycle;
	TCE0_CCA = TCE0_CCB = duty_cycle;

	while (!((leg1_status()==1 || leg2_status()==1 || leg3_status()==1) && (leg1_status()==-1 || leg2_status()==-1 || leg3_status()==-1)))
	{
		_delay_ms(200);
		if ((leg1_status()==1 || leg2_status()==1 || leg3_status()==1) && (leg1_status()==-1 || leg2_status()==-1 || leg3_status()==-1))
		break;
		
		printf("LEG 1 IS %i, LEG 2 IS %i, LEG 3 IS %i\r\n", leg1_status(), leg2_status(), leg3_status());
		hadPower = 0;
		
		for (int8_t i = 0; i < 3; i++)
		{
			if (cap_status() == 0) set_blue_led(50);
			else set_red_led(50);
			_delay_ms(30);
			led_off();
			_delay_ms(70);
		}

		switch (mot)
		{
			case (1) : TCC1.CTRLB = TC1_CCAEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCC1.CTRLB = TC_WGMODE_SS_gc; break;
			case (2) : TCC0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCC0.CTRLB = TC_WGMODE_SS_gc; break;
			case (3) : TCE0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCE0.CTRLB = TC_WGMODE_SS_gc; break;
			case (4) : TCC1.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCC1.CTRLB = TC_WGMODE_SS_gc; break;
			case (5) : TCC0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCC0.CTRLB = TC_WGMODE_SS_gc; break;
			case (6) : TCE0.CTRLB = TC0_CCBEN_bm | TC_WGMODE_SS_gc; _delay_ms(onTime); TCE0.CTRLB = TC_WGMODE_SS_gc; break;
		}
		_delay_ms(offTime);
		
		mot++;
		if (mot > 6) mot = 1;
	}
	
	if (!hadPower)
	{
		led_off(); _delay_ms(20); set_green_led(50); _delay_ms(80);
		led_off(); _delay_ms(20); set_green_led(50); _delay_ms(80);
		led_off(); _delay_ms(20); set_green_led(50); _delay_ms(80);
		led_off();
	}
	
	_delay_ms(1000);
	
	TCC0_CCA = oldtcc0cca;
	TCC0_CCB = oldtcc0ccb;
	TCC1_CCA = oldtcc1cca;
	TCC1_CCB = oldtcc1ccb;
	TCE0_CCA = oldtce0cca;
	TCE0_CCB = oldtce0ccb;

	return hadPower;
}

